import os
from launch import LaunchDescription
from launch.actions import RegisterEventHandler, ExecuteProcess
from launch.event_handlers import OnProcessExit
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def replace_package_path_in_urdf(urdf_path, package_path):
    """替换URDF中的package路径为绝对路径"""
    with open(urdf_path, 'r') as urdf_file:
        urdf_content = urdf_file.read()
    
    urdf_content = urdf_content.replace(
        'package://luck_description/', 
        package_path + '/'
    )
    
    temp_urdf_path = '/tmp/temp_luck_description.urdf'
    with open(temp_urdf_path, 'w') as temp_urdf_file:
        temp_urdf_file.write(urdf_content)
    
    return temp_urdf_path

def generate_launch_description():
    package_name = 'luck_gazebo'

    # 获取包路径
    luck_description_path = get_package_share_directory('luck_description')
    urdf_path = os.path.join(luck_description_path, 'urdf', 'luck_description.urdf')
    
    # 替换URDF中的package路径为绝对路径
    temp_urdf_path = replace_package_path_in_urdf(urdf_path, luck_description_path)

    # 启动Gazebo空世界
    gazebo = ExecuteProcess(
        cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
        output='screen'
    )

    # 将机器人模型生成到Gazebo中
    spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'luck_robot', '-file', temp_urdf_path],
        output='screen'
    )

    # 加载控制器配置
    controller_config = os.path.join(
        get_package_share_directory('luck_gazebo'),
        'config',
        'controllers.yaml'
    )

    # 使用 spawner 节点加载控制器
    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster", "-c", "/controller_manager", "--param-file", controller_config],
        output='screen'
    )

    arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["arm_controller", "-c", "/controller_manager", "--param-file", controller_config],
        output='screen'
    )

    # 调整事件处理程序，确保在模型生成后按顺序启动控制器[7](@ref)
    return LaunchDescription([
        gazebo,
        spawn_entity,
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=spawn_entity,
                on_exit=[joint_state_broadcaster_spawner],
            )
        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=joint_state_broadcaster_spawner,
                on_exit=[arm_controller_spawner],
            )
        ),
    ])
